#include "uartcomm.hpp"
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include "spdlog/spdlog.h"

UartComm::UartComm(const std::string& port, unsigned int baudrate)
    : port_(port), baudrate_(baudrate) {}

UartComm::~UartComm() {
    shutdown();
}

bool UartComm::initialize() {
    fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_ == -1) {
        spdlog::error("Failed to open UART port:: {}", port_);
        return false;
    }
    
    struct termios options;
    tcgetattr(fd_, &options);
    
    speed_t speed;
    switch (baudrate_) {
        case 9600: speed = B9600; break;
        case 57600: speed = B57600; break;
        case 115200: speed = B115200; break;
        case 230400: speed = B230400; break;
        case 460800: speed = B460800; break;
        case 500000: speed = B500000; break;
        case 576000: speed = B576000; break; 
        case 921600: speed = B921600; break;
        case 1000000: speed = B1000000; break;
        case 1152000: speed = B1152000; break;
        case 4000000: speed = B4000000; break;
        default: speed = B115200; break;
    }
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);
    
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    options.c_cflag |= (CLOCAL | CREAD); 
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cflag &= ~CRTSCTS; 
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    
    tcsetattr(fd_, TCSANOW, &options);
    
    running_ = true;
    readThread_ = std::thread(&UartComm::readThreadFunc, this);
    processThread_ = std::thread(&UartComm::processReceivedData, this);
    
    return true;
}

bool UartComm::send(const std::vector<uint8_t>& data) {
    if (fd_ == -1 || !running_) return false;
    
    ssize_t written = write(fd_, data.data(), data.size());
    return written == static_cast<ssize_t>(data.size());
}

std::vector<uint8_t> UartComm::receive() {
    std::unique_lock<std::mutex> lock(dataMutex_);
    if (receiveQueue_.empty()) {
        return {};
    }
    
    auto data = receiveQueue_.front();
    receiveQueue_.pop();
    return data;
}

void UartComm::registerCallback(DataReceivedCallback callback) {
    callback_ = std::move(callback);
}

std::string UartComm::getName() const {
    return "UART:" + port_;
}

bool UartComm::isConnected() const {
    return fd_ != -1 && running_;
}

void UartComm::shutdown() {
    running_ = false;
    
    if (readThread_.joinable()) {
        readThread_.join();     
    }
    
    if (processThread_.joinable()) {
        dataCond_.notify_all();
        processThread_.join();
    }
    
    if (fd_ != -1) {
        close(fd_);
        fd_ = -1;
    }
}

void UartComm::readThreadFunc() {
    uint8_t buffer[256];
    
    while (running_) {
        ssize_t bytesRead = read(fd_, buffer, sizeof(buffer));
        if (bytesRead > 0) {
            std::unique_lock<std::mutex> lock(dataMutex_);
            
            for (ssize_t i = 0; i < bytesRead; ++i) {
                ringBuffer_[head_] = buffer[i];
                head_ = (head_ + 1) % BUFFER_SIZE;
                
                if (head_ == tail_) {
                    tail_ = (tail_ + 1) % BUFFER_SIZE;
                }
            }
            
            dataCond_.notify_one();
        } else if (bytesRead == -1) {
            if (errno != EAGAIN && errno != EWOULDBLOCK) {
                running_ = false;
            }
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

void UartComm::processReceivedData() {
    while (running_) {
        std::unique_lock<std::mutex> lock(dataMutex_);
        dataCond_.wait(lock, [this] { return !running_ || head_ != tail_; });
        
        if (!running_) break;
        
        std::vector<uint8_t> data;
        while (tail_ != head_) {
            data.push_back(ringBuffer_[tail_]);
            tail_ = (tail_ + 1) % BUFFER_SIZE;
        }
        
        if (!data.empty()) {
            receiveQueue_.push(data);
            
            if (callback_) {
                lock.unlock();
                callback_(data);
                lock.lock();
            }
        }
    }
}